#pragma once
#include "common.h"

namespace drone_coord
{
    /**
     * @brief 相机内参结构体
     */
    struct CameraIntrinsics
    {
        double fx;     //< 焦距 x
        double fy;     //< 焦距 y
        double cx;     //< 光心 x 坐标
        double cy;     //< 光心 y 坐标
        double k1, k2; // 径向畸变系数

        CameraIntrinsics() : fx(398.9733), fy(397.3518),
                             cx(321.5679), cy(246.2837),
                             k1(-0.0933), k2(-6.7703e-04) {}
    };
    /**
     * @brief 创建基于欧拉角的旋转矩阵（ZYX顺序）
     *
     * 旋转顺序：偏航(Yaw) -> 俯仰(Pitch) -> 横滚(Roll)
     *
     * ZYX 欧拉角旋转矩阵，先绕 Z 轴（Yaw），再绕 Y 轴（Pitch），最后绕 X 轴（Roll）
     * @param yaw_deg   偏航角（度），绕Z轴旋转, 逆时针为正
     * @param pitch_deg 俯仰角（度），绕Y轴旋转，向下为负
     * @param roll_deg  横滚角（度），绕X轴旋转
     * @return          生成的3x3旋转矩阵
     */
    Matrix3x3 create_rotation_matrix(double yaw_deg, double pitch_deg, double roll_deg);

    /**
     * @brief 从像素坐标到全局坐标的完整变换
     *
     * 坐标转换链：像素坐标系 → 相机坐标系 → 机体坐标系 → 全局坐标系
     *
     * @param[in] pixel       目标像素坐标（单位：像素）
     * @param[in] global_pos  相机全局坐标位置（单位：米）
     * @param[in] drone_rot   无人机本体旋转矩阵
     * @param[in] camera_rot  相机安装旋转矩阵
     * @param[in] intrinsics  相机内参结构体
     * @param[out] result     计算得到的地面目标坐标（Z=0平面）
     * @return 是否成功找到地面交点（true表示有效结果）
     */
    bool pixel_to_global(
        const Vector3d &pixel,
        const Vector3d &global_pos,
        const Matrix3x3 &drone_rot,
        const Matrix3x3 &camera_rot,
        const CameraIntrinsics &intrinsics,
        Vector3d &result,
        Vector3d &ray_drone_out,
        double &t_out);

    /**
     * @brief 获取相机在全局坐标系中的空间位置
     *
     * 坐标变换链：无人机本体坐标系 → 全局坐标系
     * 通过无人机全局位置和相机相对位置，计算相机在全局坐标系中的绝对坐标
     *
     * @param[in] drone_pos   无人机在全局坐标系中的位置坐标（单位：米）
     * @param[in] camera_pos  相机在无人机本体坐标系中的安装偏移量（单位：米）
     * @return Vector3d       相机在全局坐标系中的三维坐标（单位：米）
     */
    Vector3d get_global_pos(Vector3d &drone_pos, Vector3d &camera_pos, Matrix3x3 &drone_rot);

} // namespace drone_coord